#include "pose_detection_post_process.h"
#include <vector>
#include <iostream>
#include <sstream>
#include <cmath>
#include <math.h>
#include <regex>
#include "hiaiengine/log.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <iostream>
#include <math.h>

using hiai::Engine;
using namespace ascend::presenter;
using namespace std::__cxx11;
using namespace std;
using namespace cv;

// register data type
HIAI_REGISTER_DATA_TYPE("EngineTransT", EngineTransT);
HIAI_REGISTER_DATA_TYPE("OutputT", OutputT);
HIAI_REGISTER_DATA_TYPE("ScaleInfoT", ScaleInfoT);
HIAI_REGISTER_DATA_TYPE("NewImageParaT", NewImageParaT);
HIAI_REGISTER_DATA_TYPE("BatchImageParaWithScaleT", BatchImageParaWithScaleT);

// constants
namespace {
  const uint32_t kModelinWidth = 224;
  const uint32_t kModelinHeight = 224;
  const uint32_t kModeloutWidth = kModelinWidth/8;
  const uint32_t kModeloutHeight = kModelinHeight/8;
  // port number range
  const int32_t kPortMinNumber = 0;
  const int32_t kPortMaxNumber = 65535;

  // confidence range
  const float kConfidenceMin = 0.0;
  const float kConfidenceMax = 1.0;

  // pose detection function return value
  const int32_t kFdFunSuccess = 0;
  const int32_t kFdFunFailed = -1;

  // need to deal results when index is 2
  const int32_t kDealResultIndex = 2;

  // each results size
  // const int32_t kEachResultSize = 7;

  // pose attribute
  const float kAttributePoseLabelValue = 1.0;
  const float kAttributePoseDeviation = 0.00001;

  // percent
  const int32_t kScorePercent = 100;

  // IP regular expression
  const std::string kIpRegularExpression =
      "^((25[0-5]|2[0-4]\\d|[1]{1}\\d{1}\\d{1}|[1-9]{1}\\d{1}|\\d{1})($|(?!\\.$)\\.)){4}$";

  // channel name regular expression
  const std::string kChannelNameRegularExpression = "[a-zA-Z0-9/]+";

  const std::vector<unsigned int> PosePairs{ 
    1, 2, 1, 5, 2, 3, 3, 4, 5, 6, 6, 7, 1, 8, 8, 9, 9, 10, 
    1, 11, 11, 12, 12, 13, 1, 0, 0, 14, 14, 16, 0, 15, 15, 17, 
    2, 16, 5, 17 };
  const std::vector<unsigned int> PoseMap{ 
    31, 32, 39, 40, 33, 34, 35, 36, 41, 42, 43, 44, 19, 20, 21, 22, 23, 24, 
    25, 26, 27, 28, 29, 30, 47, 48, 49, 50, 53, 54, 51, 52, 55, 56, 
    37, 38, 45, 46 };
  const std::vector<u_int32_t> ANGLE{1,2,3, 2,3,4, 1,5,6, 5,6,7, 8,9,10, 11,12,13};
  const std::vector<u_int32_t> DIRECTION{0,16, 0,17};

  const float MinConnectScore = 0.4;
  const int MinConnectCount = 3;
  const float SampleScoreThreshold = 0.03;
  const int MaxPeaks = 96;
  const auto KeypointsNumber = 18;
  const auto outputOffset = 3 * (MaxPeaks + 1); 
  const auto SampleCount = 10;
  const int SampleCountThreshold =9;
  const float NmsThreshold = 0.05;
}

int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop) {
  struct termios newtio, oldtio;
  if(tcgetattr( fd, &oldtio)) {
    perror("SetipSerial 1");
    return -1;
  }

  bzero( &newtio, sizeof( newtio ));
  newtio.c_cflag |= CLOCAL | CREAD;
  newtio.c_cflag &= ~CSIZE;

  switch (nBits)
  {
  case 7:
    newtio.c_cflag |= CS7;
    break;
  case 8:
    newtio.c_cflag |= CS8;
    break;
  }

  switch (nEvent)
  {
    case 'O':
      newtio.c_cflag |= PARENB;
      newtio.c_cflag |= PARODD;
      newtio.c_iflag |= (INPCK | ISTRIP);
      break;
    case 'E':
      newtio.c_iflag |= (INPCK | ISTRIP);
      newtio.c_cflag |= PARENB;
      newtio.c_cflag |= ~PARODD;
      break;
    case 'N':
      newtio.c_cflag |= ~PARENB;
      break;
  }

  switch (nSpeed)
  {
    case 9600:
      cfsetispeed(&newtio, B9600);
      cfsetospeed(&newtio, B9600);
      break;
    case 115200:
      cfsetispeed(&newtio, B115200);
      cfsetospeed(&newtio, B115200);    
      break;
  }

  if (nStop == 1)
    newtio.c_cflag &= ~CSTOPB;
  else if (nStop == 2)
    newtio.c_cflag |= CSTOPB;

  newtio.c_cc[VTIME] = 0;
  newtio.c_cc[VMIN] = 0;
  tcflush(fd, TCIFLUSH);
  if(tcsetattr(fd, TCSANOW,&newtio)) {
    perror("com set error!");
  return -1;
  }
  cout<< "set done!" << endl;

  return 0;
}

Data* create_data(int num, int channels, int height, int width){
  Data* data = new Data();
  data->num = num;
  data->width = width;
  data->channels = channels;
  data->height = height;
  data->count = num*width*channels*height;
  data->array = new float[data->count];
  data->capacity_count = data->count;
  return data;
}

PoseDetectionPostProcess::PoseDetectionPostProcess() {
  fd_post_process_config_ = nullptr;
  presenter_channel_ = nullptr;
}

HIAI_StatusT PoseDetectionPostProcess::Init(
    const hiai::AIConfig& config,
    const std::vector<hiai::AIModelDescription>& model_desc) {
  // cout<<"start post init"<<endl;
  HIAI_ENGINE_LOG("Begin initialize!");

  // get configurations
  if (fd_post_process_config_ == nullptr) {
    fd_post_process_config_ = std::make_shared<PoseDetectionPostConfig>();
  }

  // get parameters from graph.config
  for (int index = 0; index < config.items_size(); index++) {
    const ::hiai::AIConfigItem& item = config.items(index);
    const std::string& name = item.name();
    const std::string& value = item.value();
    std::stringstream ss;
    ss << value;
    if (name == "Confidence") {
      ss >> (*fd_post_process_config_).confidence;
      // validate confidence
      if (IsInvalidConfidence(fd_post_process_config_->confidence)) {
        HIAI_ENGINE_LOG(HIAI_GRAPH_INVALID_VALUE,
                        "Confidence=%s which configured is invalid.",
                        value.c_str());
        cout<<"configdenc error"<<endl;
        return HIAI_ERROR;
      }
    } else if (name == "PresenterIp") {
      // validate presenter server IP
      if (IsInValidIp(value)) {
        HIAI_ENGINE_LOG(HIAI_GRAPH_INVALID_VALUE,
                        "PresenterIp=%s which configured is invalid.",
                        value.c_str());
        cout<<"valid ip error"<<endl;
        return HIAI_ERROR;
      }
      ss >> (*fd_post_process_config_).presenter_ip;
    } else if (name == "PresenterPort") {
      ss >> (*fd_post_process_config_).presenter_port;
      // validate presenter server port
      if (IsInValidPort(fd_post_process_config_->presenter_port)) {
        HIAI_ENGINE_LOG(HIAI_GRAPH_INVALID_VALUE,
                        "PresenterPort=%s which configured is invalid.",
                        value.c_str());
        cout<<"valid port"<<endl;
        return HIAI_ERROR;
      }
    } else if (name == "ChannelName") {
      // validate channel name
      if (IsInValidChannelName(value)) {
        HIAI_ENGINE_LOG(HIAI_GRAPH_INVALID_VALUE,
                        "ChannelName=%s which configured is invalid.",
                        value.c_str());
        cout<<"valid channel"<<endl;
        return HIAI_ERROR;
      }
      ss >> (*fd_post_process_config_).channel_name;
    }
    // else : nothing need to do
  }
  // cout<<"check presenter"<<endl;
  // call presenter agent, create connection to presenter server
  uint16_t u_port = static_cast<uint16_t>(fd_post_process_config_
      ->presenter_port);
  OpenChannelParam channel_param = { fd_post_process_config_->presenter_ip,
      u_port, fd_post_process_config_->channel_name, ContentType::kVideo };
  // cout<<"higi"<<endl;
  Channel *chan = nullptr;
  PresenterErrorCode err_code = OpenChannel(chan, channel_param);
  // open channel failed
  if (err_code != PresenterErrorCode::kNone) {
    HIAI_ENGINE_LOG(HIAI_GRAPH_INIT_FAILED,
                    "Open presenter channel failed, error code=%d", err_code);
    cout<<"open channel fail"<<endl;
    return HIAI_ERROR;
  }

  presenter_channel_.reset(chan);
  HIAI_ENGINE_LOG(HIAI_DEBUG_INFO, "End initialize!");
  return HIAI_OK;
}

bool PoseDetectionPostProcess::IsInValidIp(const std::string &ip) {
  regex re(kIpRegularExpression);
  smatch sm;
  return !regex_match(ip, sm, re);
}

bool PoseDetectionPostProcess::IsInValidPort(int32_t port) {
  return (port <= kPortMinNumber) || (port > kPortMaxNumber);
}

bool PoseDetectionPostProcess::IsInValidChannelName(
    const std::string &channel_name) {
  regex re(kChannelNameRegularExpression);
  smatch sm;
  return !regex_match(channel_name, sm, re);
}

bool PoseDetectionPostProcess::IsInvalidConfidence(float confidence) {
  return (confidence <= kConfidenceMin) || (confidence > kConfidenceMax);
}

int32_t PoseDetectionPostProcess::SendImage(uint32_t height, uint32_t width,
      uint32_t size, u_int8_t *data, std::vector<DetectionResult>& detection_results){
  int32_t status = kFdFunSuccess;
  // parameter
  ImageFrame image_frame_para;
  image_frame_para.format = ImageFormat::kJpeg;
  image_frame_para.width = width;
  image_frame_para.height = height;
  image_frame_para.size = size;
  image_frame_para.data = data;
  image_frame_para.detection_results = detection_results;

  PresenterErrorCode p_ret = PresentImage(presenter_channel_.get(),
                                            image_frame_para);
  // send to presenter failed
  if (p_ret != PresenterErrorCode::kNone) {
    HIAI_ENGINE_LOG(HIAI_ENGINE_RUN_ARGS_NOT_RIGHT,
                      "Send JPEG image to presenter failed, error code=%d",
                      p_ret);
    status = kFdFunFailed;
    cout<<"presenter error"<<endl;
  }

  return status;
}

HIAI_StatusT PoseDetectionPostProcess::HandleOriginalImage(
    const std::shared_ptr<EngineTransT> &inference_res) {
  // cout<<"origin"<<endl;
  HIAI_StatusT status = HIAI_OK;
  std::vector<NewImageParaT> img_vec = inference_res->imgs;
  // dealing every original image
  for (uint32_t ind = 0; ind < inference_res->b_info.batch_size; ind++) {
    uint32_t width = img_vec[ind].img.width;
    uint32_t height = img_vec[ind].img.height;
    uint32_t size = img_vec[ind].img.size;

    // call SendImage
    // 1. call DVPP to change YUV420SP image to JPEG
    // 2. send image to presenter
    vector<DetectionResult> detection_results;
    int32_t ret = SendImage(height, width, size, img_vec[ind].img.data.get(), 
                            detection_results);
    if (ret == kFdFunFailed){
      status = HIAI_ERROR;
      continue;
    }
  }
  return status;
}

unsigned int person_flag = 0;
void ResultNms(Data* nms_in, Data* nms_out, float threshold){
  int w = nms_in->width;
  int h = nms_in->height;
  float* in_ptr = nms_in->array;
  int in_offset = w * h;
  float* out_ptr = nms_out->array;
  int out_offset = nms_out->width * nms_out->height;
  int max_peaks = nms_out->height - 1;

  for (int n = 0; n < nms_in->num; ++n){
    for (int c = 0; c < nms_in->channels - 1; ++c){
      int num_peaks = 0;
      for (int y = 1; y < h - 1 && num_peaks != max_peaks; ++y){
        for (int x = 1; x < w - 1 && num_peaks != max_peaks; ++x){
          float value = in_ptr[y*w + x];
          if (value > threshold){
            const float topLeft = in_ptr[(y - 1)*w + x - 1];
            const float top = in_ptr[(y - 1)*w + x];
            const float topRight = in_ptr[(y - 1)*w + x + 1];
            const float left = in_ptr[y*w + x - 1];
            const float right = in_ptr[y*w + x + 1];
            const float bottomLeft = in_ptr[(y + 1)*w + x - 1];
            const float bottom = in_ptr[(y + 1)*w + x];
            const float bottomRight = in_ptr[(y + 1)*w + x + 1];
            if (value > topLeft && value > top && value > topRight
              && value > left && value > right && value > bottomLeft
              && value > bottom && value > bottomRight){
              float lx = 0;
              float ly = 0;
              float total_score = 0;
              for (int kx = -3; kx <= 3; ++kx){
                int ux = x + kx;
                if (ux >= 0 && ux < w){
                  for (int ky = -3; ky <= 3; ++ky){
                    int uy = y + ky;
                    if (uy >= 0 && uy < h){
                      float score = in_ptr[uy * w + ux];
                      lx += ux * score;
                      ly += uy * score;
                      total_score += score;
                    }
                  }
                }
              }
              lx /= total_score;
              ly /= total_score;
              total_score = value;
              out_ptr[(num_peaks + 1) * 3 + 0] = lx;
              out_ptr[(num_peaks + 1) * 3 + 1] = ly;
              out_ptr[(num_peaks + 1) * 3 + 2] = total_score;
              num_peaks++;
            }
          }
        }
      }
      out_ptr[0] = num_peaks;
      in_ptr += in_offset;
      out_ptr += out_offset;
    }
  }
}

std::vector<float> ConnectKeypoints(std::vector<float> Keypoints, const float* const inputPtr, const float* const outputPtr, 
  uint32_t height , uint32_t width){
  
  const auto &pairs = PosePairs;
  const auto &mapIdx = PoseMap;
  const auto PairsCount = pairs.size()/2;
  const auto inputOffset = width*height; 
  
  std::vector<std::pair<std::vector<int>, double>> kpset;
  for (auto pairIndex = 0u; pairIndex < PairsCount; pairIndex++){
    const auto PartA = pairs[2 * pairIndex];
    const auto PartB = pairs[2 * pairIndex + 1];
    const auto* candidateA = outputPtr + PartA*outputOffset;
    const auto* candidateB = outputPtr + PartB*outputOffset;
    const auto nA = int(candidateA[0] + 0.5);
    const auto nB = int(candidateB[0] + 0.5);
    if (nA == 0 || nB == 0){
      if (nA == 0){
        for (auto i = 1; i <= nB; i++){
          bool num = false;
          const auto indexB = PartB;
          for (auto j = 0u; j < kpset.size(); j++){
            const auto off = (int)PartB*outputOffset + i * 3 + 2;
            if (kpset[j].first[indexB] == off){
              num= true;
              break;
            }
          }
          if (!num){
            std::vector<int> rowVector(KeypointsNumber+1, 0);
            rowVector[PartB] = PartB*outputOffset + i * 3 + 2;
            rowVector[KeypointsNumber] = 1;
            const auto kpsetScore = candidateB[i * 3 + 2];
            kpset.emplace_back(std::make_pair(rowVector, kpsetScore));
          }
        }
      }
      // if (nA != 0 && nB == 0)
      else{    
        for (auto i = 1; i <= nA; i++){
          bool num = false;
          const auto indexA = PartA;
          for (int j = 0; j < kpset.size(); j++){
            const auto off = PartA*outputOffset + i * 3 + 2;
            if (kpset[j].first[indexA] == off){
              num= true;
              break;
            }
          }
          if (!num){
            std::vector<int> rowVector(KeypointsNumber+1, 0);
            rowVector[PartA] = PartA*outputOffset + i * 3 + 2;
            rowVector[KeypointsNumber] = 1;
            const auto kpsetScore = candidateA[i * 3 + 2];
            kpset.emplace_back(std::make_pair(rowVector, kpsetScore));
          }
        }
      }
    }
    
    // (nA != 0 && nB != 0)
    else{
      std::vector<std::tuple<double, int, int>> temp;
      const auto* const mapX = inputPtr + mapIdx[2 * pairIndex] * inputOffset; 
      const auto* const mapY = inputPtr + mapIdx[2 * pairIndex + 1] * inputOffset;
      for (auto i = 1; i <= nA; i++){
        for (auto j = 1; j <= nB; j++){
          const auto dX = candidateB[j * 3] - candidateA[i * 3];
          const auto dY = candidateB[j * 3 + 1] - candidateA[i * 3 + 1];
          const auto normVec = float(std::sqrt(dX*dX + dY*dY));
          if (normVec > 1e-6){
            const auto sX = candidateA[i * 3];
            const auto sY = candidateA[i * 3 + 1];
            const auto cosX = dX / normVec;
            const auto cosY = dY / normVec;

            auto sum = 0.;
            auto count = 0;
            for (auto s = 0; s < SampleCount; s++){ 
              const auto one_width = int(sX + s*dX / SampleCount + 0.5);
              const auto one_height = int(sY + s*dY / SampleCount + 0.5);
              const auto mX = width - 1 <  one_width ? width-1 : one_width;
              const auto mY = height - 1 <  one_height ? height-1 : one_height;
              const int idx = mY * width + mX;
              const float score = cosX*mapX[idx] + cosY*mapY[idx];
              if (score > SampleScoreThreshold){
                sum += score;
                count++;
              }
            }
            if (count > SampleCountThreshold){
              temp.emplace_back(std::make_tuple(sum / count, i, j));
            }
          }
        }
      }
      if (!temp.empty()){
        std::sort(temp.begin(), temp.end(), std::greater<std::tuple<float, int, int>>());
      }

      std::vector<std::tuple<int, int, double>> connectionK; 
      const auto minAB = nA < nB ? nA : nB;
      std::vector<int> occurA(nA, 0);
      std::vector<int> occurB(nB, 0);
      auto counter = 0;
      for (auto row = 0u; row < temp.size(); row++){
        const auto score = std::get<0>(temp[row]);
        const auto x = std::get<1>(temp[row]);
        const auto y = std::get<2>(temp[row]);
        if (!occurA[x - 1] && !occurB[y - 1]){  
          connectionK.emplace_back(std::make_tuple(
            PartA*outputOffset + x * 3 + 2,
            PartB*outputOffset + y * 3 + 2,
            score));
          counter++;
          if (counter == minAB)
            break;
          occurA[x - 1] = 1;
          occurB[y - 1] = 1;
        }
      }

      if (pairIndex == 0){
        for (const auto connectionKI : connectionK){
          std::vector<int> rowVector(KeypointsNumber + 1, 0);
          const auto indexA = std::get<0>(connectionKI);
          const auto indexB = std::get<1>(connectionKI);
          const auto score = std::get<2>(connectionKI);
          rowVector[pairs[0]] = indexA;
          rowVector[pairs[1]] = indexB;
          rowVector[KeypointsNumber] = 2;
          const auto kpsetScore = outputPtr[indexA] + outputPtr[indexB] + score;
          kpset.emplace_back(std::make_pair(rowVector, kpsetScore));  
        }
      }
      else if (pairIndex == 17 || pairIndex == 18){
        for (const auto& connectionKI : connectionK){
          const auto indexA = std::get<0>(connectionKI);
          const auto indexB = std::get<1>(connectionKI);
          for (auto& kpsetJ : kpset){
            auto& kpSetJA =kpsetJ.first[PartA];
            auto& kpSetJB =kpsetJ.first[PartB];
            if (kpSetJB == indexA & kpSetJB == 0){
              kpSetJB = indexB;
            }
            else if (kpSetJB == indexB && kpSetJB == 0){
              kpSetJA= indexA;
            }   
          }
        }
      }
      else{
        if (!connectionK.empty()){
          for (auto i = 0u; i < connectionK.size(); i++){
            const auto indexA = std::get<0>(connectionK[i]);
            const auto indexB = std::get<1>(connectionK[i]);
            const auto score = std::get<2>(connectionK[i]);
            auto num = 0;
            for (int j = 0; j < kpset.size(); j++){
              if (kpset[j].first[PartA] == indexA){
                num++;
                kpset[j].first[PartB] = indexB;
                kpset[j].first[KeypointsNumber] = kpset[j].first[KeypointsNumber] + 1;
                kpset[j].second = kpset[j].second + outputPtr[indexB] + score;
              }
            }
            if (num == 0){
              std::vector<int> rowVector(KeypointsNumber + 1, 0);
              rowVector[PartA] = indexA;
              rowVector[PartB] = indexB;
              rowVector[KeypointsNumber] = 2;
              const auto kpsetScore = outputPtr[indexA] + outputPtr[indexB] + score;
              kpset.emplace_back(std::make_pair(rowVector, kpsetScore));
            } 
          }
        }
      }
    }
  }
  int PeopleCount = 0; 
  std::vector<int> validIndexes;
  int valid_size = MaxPeaks < kpset.size() ? MaxPeaks : kpset.size();
  validIndexes.reserve(valid_size);
  for (int index = 0; index < kpset.size(); index++){
    const int kpsetCounter = kpset[index].first[KeypointsNumber];
    const auto kpsetScore = kpset[index].second;
    if (kpsetCounter >= MinConnectCount && (kpsetScore / kpsetCounter) > MinConnectScore){
      PeopleCount++;
      validIndexes.emplace_back(index);
      if (PeopleCount == MaxPeaks)
        break;
      }
    else if (kpsetCounter < 1){
      printf("adkpsetCounter. Bug in this function if this happens. %d, %s, %s", __LINE__, __FUNCTION__, __FILE__);
    }
  } 

  // Fill and return Keypoints
  if (PeopleCount > 0){
    Keypoints.resize(PeopleCount * (int)KeypointsNumber * 3);
    person_flag = PeopleCount;
  }
  else{
    Keypoints.clear();
  }
  const float scaleFactor=1;
  for (int person = 0; person < validIndexes.size(); person++){
    const auto& kpsetI = kpset[validIndexes[person]].first;
    for (int kp = 0; kp < KeypointsNumber; kp++){
      const auto baseOffset = (person*KeypointsNumber + kp) * 3;
      const auto kpIndex = kpsetI[kp];
      if (kpIndex > 0){
        Keypoints[baseOffset] = outputPtr[kpIndex - 2] * scaleFactor;
        Keypoints[baseOffset + 1] = outputPtr[kpIndex - 1] * scaleFactor;
        Keypoints[baseOffset + 2] = outputPtr[kpIndex];
      }
      else{
        Keypoints[baseOffset] = 0.f;
        Keypoints[baseOffset + 1] = 0.f;
        Keypoints[baseOffset + 2] = 0.f;
      }
      cout<<"keypoints"<<kp<<":  ("<<Keypoints[baseOffset]<<", "<<Keypoints[baseOffset + 1]<<", "<<Keypoints[baseOffset + 2]<<")"<<endl;
    }
  }
  // cout<<"hen"<<endl;
  validIndexes.clear();
  return Keypoints;
}

//uint16_t head_temp;
vector<float> GetAngles(std::vector<float> kpy, vector<float> angles){
  
  const u_int32_t AngleCount = ANGLE.size()/3;
  // cout<<"guanjieshu"<<AngleCount<<endl;
  cout<<"钱多"<<endl;
  // cout<<"kpysize: "<<kpy.size()<<endl;

  cout<<"ko"<<endl;
  for (u_int32_t i = 0; i < 1; i++) {
    for (u_int32_t j = 0; j < AngleCount; j++) {
      // cout<<"hhhhhh"<<endl;
      float edge1x = kpy[(i*KeypointsNumber + ANGLE[j*3])*3];
      float edge1y = kpy[(i*KeypointsNumber + ANGLE[j*3])*3+1];
      float anglex = kpy[(i*KeypointsNumber + ANGLE[j*3+1])*3];
      float angley = kpy[(i*KeypointsNumber + ANGLE[j*3+1])*3+1];
      float edge2x = kpy[(i*KeypointsNumber + ANGLE[j*3+2])*3];
      float edge2y = kpy[(i*KeypointsNumber + ANGLE[j*3+2])*3+1];
      // cout<<"hhahah"<<endl;
      float dx1 = edge1x - anglex;
      float dy1 = edge1y - angley;
      float dx2 = edge2x - anglex;
      float dy2 = edge2y - angley;
      // cout<<"ddd"<<endl;
      float vec = dx1*dx2 + dy1*dy2;
      float del = sqrt((pow(dx1,2)+pow(dy1,2))*(pow(dx2,2)+pow(dy2,2)));
      float angle = (acos(vec/del)*180/3.14);
      // std::copy(angle[i], input.arr(), arr);
      cout<<"关节"<<ANGLE[j*3+1]<<"角度："<<angle<<endl;
      angles.push_back(angle);
    }
    // cout<<"endl"<<endl;
    
    float lx = kpy[(i*KeypointsNumber + DIRECTION[1])*3];
    float ly = kpy[(i*KeypointsNumber + DIRECTION[1])*3+1];
    float rx = kpy[(i*KeypointsNumber + DIRECTION[3])*3];
    float ry = kpy[(i*KeypointsNumber + DIRECTION[3])*3+1];

    float x0 = kpy[i*KeypointsNumber*3];
    float y0 = kpy[i*KeypointsNumber*3+1];

    float dl = sqrt(pow((lx-x0),2) + pow((ly-y0),2));
    float dr = sqrt(pow((rx-x0),2) + pow((ry-y0),2));
    if(dl>2*(dr+dl)/3){
      //head_temp = 45;
      cout<<"右转头"<<endl;
    }
    else if(dr>2*(dr+dl)/3){

      cout<<"左转头"<<endl;
    }
    else{
      cout<<"you can turn your head"<<endl;
    }  
  }
  kpy.clear();
  return angles;
}

void Release(Data** blob){
  if (blob){
    Data* ptr = *blob;
    if (ptr){
      if (ptr->array)
        delete[] ptr->array;
      delete ptr;
    }
    *blob = 0;
  }
}

float angle_k1_k2(float x1, float y1, float x2, float y2, float x3, float y3){
  float k1,k2,k;
  if(x2 == x1)
    k1 = 10000;
  else
    k1 = (y2 - y1)/(x2 - x1);
  
  if(x3 == x2)
    k2 = 10000;
  else
    k2 = (y3 - y2)/(x3 - x2);

  k = (k1 - k2)/(1 + k1*k2);
  float ang_temp;
  if(k1*k2 == -1)
    ang_temp = 90;
  else
    ang_temp = atan(abs(k)) * 180.0/3.1416;
  
  return ang_temp;
}

HIAI_StatusT PoseDetectionPostProcess::HandleResults(
    const std::shared_ptr<EngineTransT> &inference_res) {
  // cout<<"start handle"<<endl;
  HIAI_StatusT status = HIAI_OK;
  std::vector<NewImageParaT> img_vec = inference_res->imgs;
  std::vector<OutputT> output_data_vec = inference_res->output_datas;
  // dealing every image
  for (uint32_t ind = 0; ind < inference_res->b_info.batch_size; ind++) {
    int32_t out_index = ind * kDealResultIndex;
    OutputT out = output_data_vec[out_index];
    std::shared_ptr<hiai::AISimpleTensor> result_tensor = std::make_shared<
        hiai::AISimpleTensor>();
    result_tensor->SetBuffer(out.data.get(), out.size);
    int32_t size = result_tensor->GetSize() / sizeof(float);
    float result[size];
    errno_t mem_ret = memcpy_s(result, sizeof(result),
                               result_tensor->GetBuffer(),
                               result_tensor->GetSize());
    // memory copy failed, skip this image
    if (mem_ret != EOK) {
      cout<<"memcpy error"<<endl;
      HIAI_ENGINE_LOG(HIAI_ENGINE_RUN_ARGS_NOT_RIGHT,
                      "handle results: memcpy_s() error=%d", mem_ret);
      continue;
    }
    
    uint32_t width = img_vec[ind].img.width;
    uint32_t height = img_vec[ind].img.height;
    uint32_t img_size = img_vec[ind].img.size;

    Data* nms_in = create_data(1, 57, kModelinHeight, kModelinWidth);
    Data* nms_out = create_data(1, 56, MaxPeaks+1, 3);
    cv::Size basesize = cv::Size(kModelinWidth, kModelinHeight);
    for (int i = 0; i < 57; i++) {
      cv::Mat midd(kModelinHeight, kModelinWidth, CV_32F, nms_in->array + kModelinHeight*kModelinWidth*i);
      cv::Mat seni(kModeloutHeight, kModeloutWidth, CV_32F, result + kModeloutHeight*kModeloutWidth*i);
      cv::resize(seni, midd, basesize, 0, 0, CV_INTER_CUBIC);
    }
    // cout<<"start nms"<<endl;
    ResultNms(nms_in, nms_out, NmsThreshold);
    vector<float> Keypoints(MaxPeaks*KeypointsNumber*3);
    vector<float> Angles(6,0);
    Keypoints = ConnectKeypoints(Keypoints, nms_in->array, nms_out->array, kModelinHeight, kModelinWidth);
    if (Keypoints.size()>=1){
      Angles = GetAngles(Keypoints, Angles);
    }
    else
    {
      person_flag = 0;
    }
    
    int nodes_num = 18;
    float xdata[18];
    float ydata[18];
    float angdata[18];
    int xy_len = 3;
    int angles_temp[nodes_num]; 
    for(int i = 0; i < nodes_num; i ++){
        angles_temp[i] = 1500;  
    }
    
    for(int i = 0; i < nodes_num; i ++){
      xdata[i] = (int)Keypoints[i*3+0];
      //cout<<"xdata[i]"<<xdata[i]<<endl;
      printf("x:%f\n",xdata[i]);
      ydata[i] = (int)Keypoints[i*3+1];
      printf("y:%f\n",ydata[i]);
      //cout<<"ydata[i]"<<ydata[i]<<endl;
    }

    /////////////////////////////////// node data dispose ////////////////////////
    float xy_angs[nodes_num];
    int start_flag;

    for(int i = 1; i < 18; i ++){
      if(xdata[i] != 0 && ydata[i] != 0)
        ++start_flag;
      else
        start_flag = 0;   
    }

    float xn = 20;
    float yn = 20;
    if(person_flag > 0 &&  start_flag > 7){

      //node 0 head
      float temp_angs0;
      temp_angs0 = angle_k1_k2((xdata[2] + xdata[5])/2,(ydata[2] + ydata[5])/2,(xdata[14] + xdata[15])/2,(ydata[14] + ydata[15])/2,xdata[0],ydata[0]);;
      if((xdata[2] + xdata[5])/2 < xdata[0])
        xy_angs[0] = 90 + temp_angs0;
      else if((xdata[2] + xdata[5])/2 > xdata[0])
        xy_angs[0] = 90 - temp_angs0;
      else
        xy_angs[0] = 90;

      //node 3 angle, 机器左肩 = 人右肩
      float temp_ang3;
      temp_ang3 = angle_k1_k2((xdata[2] + xdata[5])/2,(ydata[2] + ydata[5])/2,xdata[2],ydata[2],xdata[3],ydata[3]);
      if(ydata[3] > ydata[2])
        xy_angs[3] = 90 - (int)temp_ang3;
      else if (ydata[3] < ydata[2])
        xy_angs[3] = 90 + (int)temp_ang3;
      else
        xy_angs[3] = 90;
      
      //node 4 angle，左臂
      float temp_ang4;
      temp_ang4 = angle_k1_k2((xdata[2] + xdata[5])/2,(ydata[2] + ydata[5])/2,xdata[3],ydata[3],xdata[4],ydata[4]);
      if(ydata[4] > ydata[3])
        xy_angs[4] = 90 - (int)temp_ang4;
      else if (ydata[4] < ydata[3])
        xy_angs[4] = 90 + (int)temp_ang4;
      else
        xy_angs[4] = 90;

      //node 6 angle，右肩
      float temp_ang6;
      temp_ang6 = angle_k1_k2((xdata[2] + xdata[5])/2,(ydata[2] + ydata[5])/2,xdata[5],ydata[5],xdata[6],ydata[6]);
      if(ydata[6] < ydata[5])
        xy_angs[6] = 90 - (int)temp_ang6; //非标
      else if (ydata[6] > ydata[5])
        xy_angs[6] = 90 + (int)temp_ang6;
      else
        xy_angs[6] = 90;
      
      //node 7 angle，右臂
      float temp_ang7;
      temp_ang7 = angle_k1_k2((xdata[2] + xdata[5])/2,(ydata[2] + ydata[5])/2,xdata[6],ydata[6],xdata[7],ydata[7]);
      if(ydata[7] < ydata[6])
        xy_angs[7] = 90 - (int)temp_ang7;
      else if (ydata[7] > ydata[6])
        xy_angs[7] = 90 + (int)temp_ang7;
      else
        xy_angs[7] = 90;

      //检测腿的位置，然后动臀的角度
      //node 9 angle，左臀
      float temp_ang9;
      temp_ang9 = angle_k1_k2(xdata[8],ydata[8],xdata[8],ydata[8],xdata[9],ydata[9]);
      if(xdata[9] < xdata[8])
        xy_angs[9] = 90 - (int)(temp_ang9);
      else
        xy_angs[9] = 90;
      
      //if(xy_angs[8] > 90 || xy_angs[8] < 0)
        //xy_angs[8] = 90;

      //node 12 angle，右臀
      float temp_ang12;
      temp_ang12 = angle_k1_k2(xdata[11],ydata[11],xdata[11],ydata[11],xdata[12],ydata[12]);
      if(xdata[12] > xdata[11])
        xy_angs[12] = 90 + (int)(temp_ang12);
      else
        xy_angs[12] = 90;
      
      float kx = 11.1111;
      float by = 500.00; 
      //for(int i = 0; i < nodes_num; i++){
        //angles_temp[i] = (int)(kx*xy_angs[3+i] + by);
      //}
      angles_temp[0] = (int)(kx*xy_angs[3] + by);
      angles_temp[1] = (int)(kx*xy_angs[4] + by);
      angles_temp[2] = (int)(kx*xy_angs[6] + by);
      angles_temp[3] = (int)(kx*xy_angs[7] + by);

      angles_temp[4] = (int)(kx*xy_angs[0] + by);
      angles_temp[5] = (int)(kx*xy_angs[9] + by);
      angles_temp[6] = (int)(kx*xy_angs[12] + by);

    }
    else
    {
      for(int i = 0; i < nodes_num; i ++){
        angles_temp[i] = 1500;  
      }
    }

    for(int i = 0; i < nodes_num-1; i ++){
        cout<<"the "<< i << "angles_temp is "<<angles_temp[i]<<endl;  
      }
    
    ///////////////////////////////// node data dispose end ///////////////////////

    Keypoints.clear();
    Angles.clear();
    cout<<"ok"<<endl;

    
    int len_angle = 28;//22
    int len_buf = 14;
    //数据发送缓存
    unsigned short angel_hex[len_angle];
    unsigned short angel_hex_out[len_buf];
    
    angel_hex[0] = 85;  //格式：0x55 0x55开头
    angel_hex[1] = 85;  
    angel_hex[2] = 28;  //0x08  data length: 3*x+5
    angel_hex[3] = 3;  //0xA1指令值 
    angel_hex[4] = 7;  //0x01舵机个数
    angel_hex[5] = 50;  //0xf4,0x01时间低和时间高
    angel_hex[6] = 0;
    
    for(int i = 0; i < nodes_num-1; i++){
      angel_hex[8+i*3] = angles_temp[i] & 0x00ff;
      angel_hex[8+i*3+1] = (angles_temp[i] >> 8) & 0x00ff;
    }
  
    angel_hex[7] = 10;
    angel_hex[10] = 9;
    angel_hex[13] = 13;
    angel_hex[16] = 14;
    angel_hex[19] = 8;
    angel_hex[22] = 4;
    angel_hex[25] = 19;

    for(int i = 0; i < 14; i++){
      //angel_hex_out[i] = angel_hex[i];
      angel_hex_out[i] = (angel_hex[i*2+0]) + (angel_hex[i*2+1] << 8);
      //cout<<"angel_hex[i*2+0]"<<angel_hex[i*2+0]<<endl;
      //cout<<"angel_hex[i*2+1]"<<angel_hex[i*2+1]<<endl;
      //cout<<"out:" <<angel_hex_out[i] <<endl;
    }
    
    ///////////////////////////  uart data send ////////////////////////////////////////////
    int  fd1, nset, rett;

    //unsigned char test_data[10] = {0x55,0x55,0x08,0xA1,0x01,0xF4,0x01,0x08,0xc4,0xff};
    fd1 = open("/dev/ttyAMA0",O_RDWR | O_NOCTTY); //O_RDWR | O_NOCTTY
    if(fd1 == -1){
      cout<<"no such ttyAMA0 exist!" <<endl;
      exit(1);
    }
    cout<<"ttyAMA0 open succees!" <<endl;

    nset = set_opt(fd1, 115200, 8, 'N', 1);
    if(nset == -1){
      exit(-1);
    }

    cout<<"ttyAMA0 set success!" <<endl;

    rett = write(fd1, angel_hex_out, 28);
    //rett = write(fd1, test_data, 10);
    if(rett > 0){
      cout<< "write to ttyAMA0 success!" << endl;
    }
    //sleep(0.1);//5*1000＝延时5秒
    close(fd1);  
    //++mov_count;
    
    cout<< "ttyAMA0 closed success!" << endl;
    
    Release(&nms_in);
    Release(&nms_out);

    std::vector<DetectionResult> detection_results;
    int32_t ret;
    ret = SendImage(height, width, img_size, img_vec[ind].img.data.get(), 
                    detection_results);
    // check send result
    if (ret == kFdFunFailed) {
      status = HIAI_ERROR;
    }
    // cout<<"pic: "<<ind<<endl;
  }
  // cout<<"keep gong along this road"<<endl;

  return status;
}

HIAI_IMPL_ENGINE_PROCESS("pose_detection_post_process",
  PoseDetectionPostProcess, INPUT_SIZE) {
  // check arg0 is null or not
  // cout<<"hello"<<endl;
  if (arg0 == nullptr) {
    HIAI_ENGINE_LOG(HIAI_ENGINE_RUN_ARGS_NOT_RIGHT,
                    "Failed to process invalid message.");
    cout<<"arg0 empty"<<endl;
    return HIAI_ERROR;
  }

  // check original image is empty or not
  std::shared_ptr<EngineTransT> inference_res = std::static_pointer_cast<
      EngineTransT>(arg0);
  if (inference_res->imgs.empty()) {
    HIAI_ENGINE_LOG(
        HIAI_ENGINE_RUN_ARGS_NOT_RIGHT,
        "Failed to process invalid message, original image is null.");
    cout<<"img_empty"<<endl;
    return HIAI_ERROR;
  }
  
  // inference failed, dealing original images
  if (!inference_res->status) {
    HIAI_ENGINE_LOG(HIAI_OK, inference_res->msg.c_str());
    HIAI_ENGINE_LOG(HIAI_OK, "will handle original image.");
    cout<<"inference fail"<<endl;
    return HandleOriginalImage(inference_res);
  }
  // inference success, dealing inference results
  // cout<<"handle inference"<<endl;
  return HandleResults(inference_res);
}
